import UR10_toolbox as ik


## get tip position 
q = [4.591084957122803, -1.9162305037127894, -1.4534547964679163, -0.7343018690692347, 2.0838029384613037, 2.5636677742004395]
tool = [0.02974172,-0.21578594, 0.38169355]
T = ik.UR10_FK(q,tool)
point = T[0:3,3]


# get orientiation
q1 = [4.631085395812988, -1.9942668120013636, -1.4051769415484827, -0.7092083136187952, 1.7361408472061157, 2.5960562229156494]
q2 = [4.557792663574219, -1.9779685179339808, -1.3891361395465296, -0.877594296132223, 1.7297810316085815, 2.595576524734497]
q3 = [4.79889784 ,-1.91541873 ,-1.38908672, -0.89679232,  1.66578005 , 2.55958442]

T1 = ik.UR10_FK(q1,tool)
T2 = ik.UR10_FK(q2,tool)
T3 = ik.UR10_FK(q3,tool)

T1[0:3,3] =point
T2[0:3,3] =point
T3[0:3,3] =point

## find angle
print(ik.UR10_IK(q3,T3,tool))
print(ik.UR10_IK(q3,T2,tool))
print(ik.UR10_IK(q3,T1,tool))

